#include "nti_matrix.h"

#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <complex>
#include <fstream>
//#include <D:\armadillo-9.200.6\include\armadillo>

//NAMESPACE_MAT_BEGIN
using namespace MAT;
using namespace std;

#if defined _MSC_VER

void MAT::WritetoFile(const zArray3d & A, const char * szfile)
{
	ofstream file(szfile);
	file << A << endl;
}

std::ostream & MAT::operator<<(std::ostream & os, const zArray3d & A)
{
	os << A.nrow<<"\t" << A.ncol<<"\t" << A.nlen << endl;
	for (unsigned int k = 0; k < A.nlen; k++)
	{
		os << k << endl;
		for (unsigned int i = 0; i<A.nrow; i++)
		{
			for (unsigned int j = 0; j<A.ncol; j++)
			{
				os << A.pdata[i][j][k]<< "\t";
			}
			os << std::endl;
		}
		os << std::endl;
	}
	return os;
}
std::ostream & MAT::operator<<(std::ostream & os, const rArray2d & A)
{
	os << A.nrow << "\t" << A.ncol << endl;
	for (unsigned int i = 0; i < A.nrow; i++)
	{
		for (unsigned int j = 0; j < A.ncol; j++)
		{
			os << A.pdata[i][j] << "\t";
		}
		os << std::endl;
	}
	os << std::endl;
	return os;
}

std::ostream & MAT::operator<<(std::ostream & os, const zArray2d & A)
{
	os << A.nrow << "\t" << A.ncol << endl;
	for (unsigned int i = 0; i < A.nrow; i++)
	{
		for (unsigned int j = 0; j < A.ncol; j++)
		{
			os << A.pdata[i][j] << "\t";
		}
		os << std::endl;
	}
	os << std::endl;
	return os;
}


#endif

//void MAT::Array1dAlloc(rArray1d & Ary, unsigned int nLen)
//{
//	assert(!Ary.pdata);
//	Ary.nlen = nLen;
//	Ary.pdata= (REAL*)::malloc(nLen*sizeof(REAL));
//}
//
//void MAT::Array1dAlloc(zArray1d & Ary, unsigned int nLen)
//{
//	assert(!Ary.pdata);
//	Ary.nlen = nLen;
//	Ary.pdata = (Cplx*)::malloc(nLen * sizeof(Cplx));
//}

rArray1d MAT::Array1dAllocR(unsigned int nLen)
{
	REAL* p= (REAL*)::malloc(nLen * sizeof(REAL));
	return rArray1d(nLen, p);
}

zArray1d MAT::Array1dAllocZ(unsigned int nLen)
{
	Cplx* p = (Cplx*)::malloc(nLen * sizeof(Cplx));
	return zArray1d(nLen, p);
}

//void MAT::Array1dAlloc(zArray1d & Ary, unsigned int nLen)
//{
//	assert(!Ary.pdata);
//	Ary.nlen = nLen;
//	Ary.pdata = (Cplx*)::malloc(nLen * sizeof(Cplx));
//}


void MAT::ArrayFree(rArray1d & Ary)
{
	if (!Ary.pdata)
	{
		return;
	}
	::free(Ary.pdata);
	Ary.nlen = 0;
	Ary.pdata = NULL;
}

void MAT::ArrayFree(zArray1d & Ary)
{
	if (!Ary.pdata)
	{
		return;
	}
	::free(Ary.pdata);
	Ary.nlen = 0;
	Ary.pdata = NULL;
}

void MAT::Array1dSet(rArray1d & Ary, const REAL & val)
{
	for (unsigned int i = 0; i < Ary.nlen; i++)
	{
		Ary.pdata[i] = val;
	}
}

void MAT::Array1dSet(zArray1d & Ary, const Cplx & val)
{
	for (unsigned int i = 0; i < Ary.nlen; i++)
	{
		Ary.pdata[i] = val;
	}
}

//void MAT::Array1dResize(rArray1d & Ary, unsigned int nLen, const REAL & val)
//{
//	if (Ary.nlen != nLen)
//	{
//		ArrayFree(Ary);
//		//Array1dAlloc(Ary, nLen);
//		Ary = Array1dAllocR(nLen);
//	}
//	for (unsigned int i = 0; i < nLen; i++)
//	{
//		Ary.pdata[i] = val;
//	}
//}
//
//void MAT::Array1dResize(zArray1d & Ary, unsigned int nLen, const Cplx & val)
//{
//	if (Ary.nlen != nLen)
//	{
//		ArrayFree(Ary);
//		Ary = Array1dAllocZ(nLen);
//		//Array1dAlloc(Ary, nLen);
//	}
//	for (unsigned int i = 0; i < nLen; i++)
//	{
//		Ary.pdata[i] = val;
//	}
//}

void MAT::Array1dCopy(const rArray1d & Asrc, rArray1d & Adst)
{
	assert(Asrc.nlen == Adst.nlen);
	//if (!IsSameDim(Asrc, Adst))
	//{
	//	Array1dResize(Adst, Asrc.nlen);
	//}
	memcpy(Adst.pdata, Asrc.pdata, Asrc.nlen * sizeof(REAL));
	//for (unsigned int i = 0; i < Asrc.nlen; i++)
	//{
	//	Adst.pdata[i] = Asrc.pdata[i];
	//}
}

void MAT::Array1dCopy(const zArray1d & Asrc, zArray1d & Adst)
{
	assert(IsSameDim(Asrc, Adst));
	//if (!IsSameDim(Asrc, Adst))
	//{
	//	Array1dResize(Adst, Asrc.nlen);
	//}
	memcpy(Adst.pdata, Asrc.pdata, Asrc.nlen * sizeof(Cplx));
}

void MAT::Array1dCopy(const REAL * pData, rArray1d & Adst)
{
	assert(Adst.pdata);
	memcpy(Adst.pdata, pData, Adst.nlen * sizeof(REAL));
}

void MAT::Array1dCopy(const REAL * pData, zArray1d & Adst)
{
	assert(Adst.pdata);
	for (unsigned int i = 0; i < Adst.nlen; i++)
	{
		Adst.pdata[i]._Val[0] = pData[i];
		Adst.pdata[i]._Val[1] = 0;
	}
}

void MAT::Array1dCopy(const Cplx * pData, zArray1d & Adst)
{
	assert(Adst.pdata);
	memcpy(Adst.pdata, pData, Adst.nlen * sizeof(Cplx));
}


void MAT::Array1dMpy3(const REAL * p, const zArray1d & A2, zArray1d & A3)
{
	assert(IsSameDim(A2, A3));
	//Array1dResize(A3, A2.nlen);
	for (unsigned int i = 0; i < A2.nlen; i++)
	{
		A3.pdata[i] = p[i] * A2.pdata[i];
	}
}

void MAT::Array1dMpy3(const zArray1d & A1, const zArray1d & A2, zArray1d & A3)
{
	assert(A1.nlen == A2.nlen);
	assert(IsSameDim(A2, A3));
	//Array1dResize(A3, A1.nlen);
	for (unsigned int i = 0; i < A1.nlen; i++)
	{
		A3.pdata[i] = A1.pdata[i] * A2.pdata[i];
	}

}

void MAT::Array1dMpy1(zArray1d & A1, const REAL & k)
{
	for (unsigned int i = 0; i < A1.nlen; i++)
	{
		A1.pdata[i] *= k;
	}
}

void MAT::Array1dMpy1(zArray1d & A1, const Cplx & k)
{
	for (unsigned int i = 0; i < A1.nlen; i++)
	{
		A1.pdata[i] *= k;
	}
}

void MAT::Array1dMpy2(const zArray1d & A1, zArray1d & A2)
{
	assert(A1.nlen == A2.nlen);
	for (unsigned int i = 0; i < A1.nlen; i++)
	{
		A2.pdata[i] *= A1.pdata[i];
	}
}

void MAT::Array1dMpy2(const REAL *p, zArray1d & A2)
{
	for (unsigned int i = 0; i < A2.nlen; i++)
	{
		A2.pdata[i] *= p[i];
	}
}

void MAT::Array1dPow2(const zArray1d & A1, const REAL & p, zArray1d & A3)
{
	//Array1dResize(A3, A1.nlen);
	assert(IsSameDim(A1, A3));

	for (unsigned int i = 0; i < A1.nlen; i++)
	{
		A3.pdata[i] = std::pow(A1.pdata[i], p);
	}
}

void MAT::Array1dPow1(zArray1d & A1, const REAL & p)
{
	for (unsigned int i = 0; i < A1.nlen; i++)
	{
		A1.pdata[i] = std::pow(A1.pdata[i], p);
	}
}

void MAT::Array1dExp2(const zArray1d & A1, zArray1d & A3)
{
	//Array1dResize(A3, A1.nlen);
	assert(IsSameDim(A1, A3));

	for (unsigned int i = 0; i < A1.nlen; i++)
	{
		A3.pdata[i] = std::exp(A1.pdata[i]);
	}
}

void MAT::Array1dExp1(zArray1d & A1)
{
	for (unsigned int i = 0; i < A1.nlen; i++)
	{
		A1.pdata[i] = std::exp(A1.pdata[i]);
	}
}

void MAT::Array1dConj2(const zArray1d & A1, zArray1d & A2)
{
	//Array1dResize(A2, A1.nlen);
	assert(IsSameDim(A1, A2));

	for (unsigned int i = 0; i < A1.nlen; i++)
	{
		A2.pdata[i] = std::conj(A1.pdata[i]);
	}
}

rArray2d MAT::Array2dAllocR(unsigned int nRow, unsigned int nCol)
{
	REAL** dM2 = (REAL**)::malloc(nRow * sizeof(REAL*));
	for (unsigned int i = 0; i < nRow; i++)
	{
		dM2[i] = (REAL*)::malloc(nCol* sizeof(REAL));
		memset(dM2[i], 0, sizeof(REAL)*nCol);
	}
	return rArray2d(nRow,nCol,dM2);
}

zArray2d MAT::Array2dAllocZ(unsigned int nRow, unsigned int nCol)
{
	Cplx** dM2 = (Cplx**)::malloc(nRow * sizeof(Cplx*));
	for (unsigned int i = 0; i < nRow; i++)
	{
		dM2[i] = (Cplx*)::malloc(nCol * sizeof(Cplx));
		memset(dM2[i], 0, sizeof(Cplx)*nCol);
	}
	return zArray2d(nRow, nCol, dM2);
}

void MAT::ArrayFree(rArray2d & Ary)
{
	for (unsigned int i = 0; i < Ary.nrow; i++)
	{
		free(Ary.pdata[i]);
	}
	free(Ary.pdata);
	Ary.nrow = 0;
	Ary.ncol = 0;
	Ary.pdata = NULL;
}

void MAT::ArrayFree(zArray2d & Ary)
{
	for (unsigned int i = 0; i < Ary.nrow; i++)
	{
		free(Ary.pdata[i]);
	}
	free(Ary.pdata);
	Ary.nrow = 0;
	Ary.ncol = 0;
	Ary.pdata = NULL;
}

rMat * MAT::MatCreate(rMat * A, int nRow, int nCol)
{
	return nullptr;
}

rArray2d * MAT::Array2dEyeZeros(rArray2d * Ary)
{
	for (unsigned int i = 0; i < Ary->nrow; i++)
	{
		memset(Ary->pdata[i], 0, Ary->ncol * sizeof(REAL));
	}
	return Ary;
}

zArray2d * MAT::Array2dEyeZeros(zArray2d * Ary)
{
	for (unsigned int i = 0; i < Ary->nrow; i++)
	{
		memset(Ary->pdata[i], 0, Ary->ncol * sizeof(Cplx));
	}
	return Ary;
}

rArray2d* MAT::Array2dEye(rArray2d* Ary)
{
	for (unsigned int i = 0; i < __min(Ary->nrow, Ary->ncol); i++)
	{
		Ary->pdata[i][i] = 1;
	}
	return Ary;
}

zArray2d* MAT::Array2dEye(zArray2d* Ary)
{
	for (unsigned int i = 0; i < __min(Ary->nrow, Ary->ncol); i++)
	{
		Ary->pdata[i][i] = R2Z(1);
	}
	return Ary;
}

rArray2d*  MAT::Array2dCatHori3(const rArray2d* A1, const rArray2d* A2, rArray2d* A3)
{
	assert(A1->nrow == A2->nrow && A1->nrow == A3->nrow && A1->ncol + A2->ncol == A3->ncol);
	REAL* psrc = NULL;
	REAL* pdst = NULL;
	for (unsigned int i = 0; i < (*A1).nrow; i++)
	{
		pdst = A3->pdata[i];
		psrc = A1->pdata[i];
		memcpy(pdst, psrc, sizeof(REAL)*A1->ncol);
		pdst += A1->ncol;
		psrc = A2->pdata[i];
		memcpy(pdst, psrc, sizeof(REAL)*A2->ncol);
	}
	return A3;
}

zArray2d* MAT::Array2dCatHori3(const zArray2d* A1, const zArray2d* A2, zArray2d* A3)
{
	assert(A1->nrow == A2->nrow && A1->nrow == A3->nrow && A1->ncol + A2->ncol == A3->ncol);
	Cplx* psrc = NULL;
	Cplx* pdst = NULL;
	for (unsigned int i = 0; i < A1->nrow; i++)
	{
		pdst = A3->pdata[i];
		psrc = A1->pdata[i];
		memcpy(pdst, psrc, sizeof(Cplx)*A1->ncol);
		pdst += A1->ncol;
		psrc = A2->pdata[i];
		memcpy(pdst, psrc, sizeof(Cplx)*A2->ncol);
	}
	return A3;
}

rArray2d * MAT::Array2dInv2(const rArray2d * A1, rArray2d * A2 )
{
	assert(IsSameDim(*A1, *A2));
	assert(A1->ncol == A1->nrow);

	rArray2d mAug = MAT::Array2dAllocR(A1->nrow, A1->ncol * 2);//augmented matrix 
	rArray2d mI = MAT::Array2dAllocR(A1->nrow, A1->ncol);
	MAT::Array2dCatHori3(A1, &mI, &mAug);
	
	int order = A1->nrow;
	for (int i = order - 1; i > 0; i--)
	{

		// Swapping each and every element of the two rows 
		// Directly swapping the rows using pointers saves time 

		if (mAug.pdata[i - 1][0] < mAug.pdata[i][0])
		{
			REAL* temp = mAug.pdata[i];
			mAug.pdata[i] = mAug.pdata[i - 1];
			mAug.pdata[i - 1] = temp;
		}
	}
	// Replace a row by sum of itself and a 
	// constant multiple of another row of the matrix 
	for (int i = 0; i < order; i++)
	{
		for (int j = 0; j < order; j++)
		{
			if (j != i)
			{
				REAL temp = mAug.pdata[j][i] / mAug.pdata[i][i];
				for (int k = 0; k < 2 * order; k++)
				{
					mAug.pdata[j][k] -= mAug.pdata[i][k] * temp;
				}
			}
		}
	}

	// Multiply each row by a nonzero integer. 
	// Divide row element by the diagonal element 
	for (int i = 0; i < order; i++)
	{
		REAL temp = mAug.pdata[i][i];
		for (int j = 0; j < 2 * order; j++)
		{

			mAug.pdata[i][j] = mAug.pdata[i][j] / temp;
		}
	}
	for (int i = 0; i < order; i++)
	{
		memcpy(A2->pdata[i], mAug.pdata[i], order * sizeof(REAL));
	}
	
	MAT::ArrayFree(mI);
	MAT::ArrayFree(mAug);
	return A2;
}


rArray3d MAT::Array3dAllocR(unsigned int nRow, unsigned int nCol, unsigned int nLen)
{
	REAL*** dM2 = (REAL***)::malloc(nRow * sizeof(REAL**));
	for (unsigned int idim = 0; idim < nRow; idim++)
	{
		dM2[idim] = (REAL**) ::malloc(nCol * sizeof(REAL*));
	}
	for (unsigned int idim = 0; idim < nRow; idim++)
	{
		for (unsigned int jdim = 0; jdim < nCol; jdim++)
		{
			dM2[idim][jdim] = (REAL*) ::malloc(nLen*sizeof(REAL));
			::memset(dM2[idim][jdim], 0, nLen*sizeof(REAL));
		}
	}
	return rArray3d(nRow, nCol, nLen, dM2);
}

zArray3d MAT::Array3dAllocZ(unsigned int nRow, unsigned int nCol, unsigned int nLen)
{
	Cplx*** dM2 = (Cplx***)::malloc(nRow * sizeof(Cplx**));
	for (unsigned int idim = 0; idim < nRow; idim++)
	{
		dM2[idim] = (Cplx**) ::malloc(nCol * sizeof(Cplx*));
	}
	for (unsigned int idim = 0; idim < nRow; idim++)
	{
		for (unsigned int jdim = 0; jdim < nCol; jdim++)
		{
			dM2[idim][jdim] = (Cplx*) ::malloc(nLen * sizeof(Cplx));
			::memset(dM2[idim][jdim], 0, nLen * sizeof(Cplx));
		}
	}
	return zArray3d(nRow, nCol, nLen, dM2);
}

void MAT::ArrayFree(rArray3d & Ary)
{

	if (!Ary.pdata)
	{
		return;
	}
	for (unsigned int idim = 0; idim < Ary.nrow; idim++)
	{
		for (unsigned int jdim = 0; jdim < Ary.ncol; jdim++)
		{
			::free(Ary.pdata[idim][jdim]);
		}
	}
	for (unsigned int idim = 0; idim < Ary.nrow; idim++)
	{
		::free(Ary.pdata[idim]);
	}
	::free(Ary.pdata);
	Ary.pdata = NULL;
	Ary.nrow = 0;
	Ary.ncol = 0;
	Ary.nlen = 0;
}

void MAT::ArrayFree(zArray3d & Ary)
{
	if (!Ary.pdata)
	{
		return;
	}
	for (unsigned int idim = 0; idim < Ary.nrow; idim++)
	{
		for (unsigned int jdim = 0; jdim < Ary.ncol; jdim++)
		{
			::free(Ary.pdata[idim][jdim]);
		}
	}
	for (unsigned int idim = 0; idim < Ary.nrow; idim++)
	{
		::free(Ary.pdata[idim]);
	}
	::free(Ary.pdata);
	Ary.pdata = NULL;
	Ary.nrow = 0;
	Ary.ncol = 0;
	Ary.nlen = 0;
}

void MAT::Array3dSet(rArray3d & Ary, const REAL & val)
{
	for (unsigned int i = 0; i < Ary.nrow; i++)
	{
		for (unsigned int j = 0; j < Ary.ncol; j++)
		{
			for (unsigned int k = 0; k < Ary.nlen; k++)
			{
				Ary.pdata[i][j][k] = val;
			}
		}
	}
}

void MAT::Array3dSet(zArray3d & Ary, const Cplx & val)
{
	for (unsigned int i = 0; i < Ary.nrow; i++)
	{
		for (unsigned int j = 0; j < Ary.ncol; j++)
		{
			for (unsigned int k = 0; k < Ary.nlen; k++)
			{
				Ary.pdata[i][j][k] = val;
			}
		}
	}
}

void MAT::Array3dResize(rArray3d & Ary, unsigned int nRow, unsigned int nCol, unsigned int nLen, const REAL& val)
{
	if (Ary.nrow != nRow || Ary.ncol != nCol || Ary.nlen != nLen)
	{
		ArrayFree(Ary);
		Ary=Array3dAllocR(nRow, nCol, nLen);
	}
	Array3dSet(Ary, val);
}

void MAT::Array3dResize(zArray3d & Ary, unsigned int nRow, unsigned int nCol, unsigned int nLen, const Cplx & val)
{
	if (Ary.nrow != nRow || Ary.ncol != nCol || Ary.nlen != nLen)
	{
		if (Ary.pdata)
		{
			ArrayFree(Ary);
		}
		Ary = Array3dAllocZ(nRow, nCol, nLen);
	}
	Array3dSet(Ary, val);
}

void MAT::Array3dCopy(const rArray3d & Asrc, rArray3d & Adst)
{
	Array3dResize(Adst, Asrc.nrow, Asrc.ncol, Asrc.nlen);
	for (unsigned int i = 0; i < Asrc.nrow; i++)
	{
		for (unsigned int j = 0; j < Asrc.ncol; j++)
		{
			memcpy(Adst.pdata[i][j], Asrc.pdata[i][j], Asrc.nlen * sizeof(REAL));
		}
	}
}

void MAT::Array3dCopy(const zArray3d & Asrc, zArray3d & Adst)
{
	Array3dResize(Adst, Asrc.nrow, Asrc.ncol, Asrc.nlen);
	for (unsigned int i = 0; i < Asrc.nrow; i++)
	{
		for (unsigned int j = 0; j < Asrc.ncol; j++)
		{
			memcpy(Adst.pdata[i][j], Asrc.pdata[i][j], Asrc.nlen * sizeof(Cplx));
		}
	}
}
